#include "sonar_driver.h"

#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>

#include <uORB/topics/parameter_update.h>


int SonarDriver::print_status()
{
    PX4_INFO("Running");
    // TODO: print additional runtime information about the state of the module

    return 0;
}

int SonarDriver::custom_command(int argc, char *argv[])
{
    /*
    if (!is_running()) {
        print_usage("not running");
        return 1;
    }

    // additional custom commands can be handled like this:
    if (!strcmp(argv[0], "do-something")) {
        get_instance()->do_something();
        return 0;
    }
     */

    return print_usage("unknown command");
}


int SonarDriver::task_spawn(int argc, char *argv[])
{
    _task_id = px4_task_spawn_cmd("module",
                                  SCHED_DEFAULT,
                                  SCHED_PRIORITY_DEFAULT,
                                  1024,
                                  (px4_main_t)&run_trampoline,
                                  (char *const *)argv);

    if (_task_id < 0)
    {
        _task_id = -1;
        return -errno;
    }

    return 0;
}

SonarDriver *SonarDriver::instantiate(int argc, char *argv[])
{
    int example_param = 0;
    bool example_flag = false;
    bool error_flag = false;

    int myoptind = 1;
    int ch;
    const char *myoptarg = nullptr;

    // parse CLI arguments
    while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF)
    {
        switch (ch)
        {
            case 'p':
                example_param = (int)strtol(myoptarg, nullptr, 10);
                break;

            case 'f':
                example_flag = true;
                break;

            case '?':
                error_flag = true;
                break;

            default:
                PX4_WARN("unrecognized flag");
                error_flag = true;
                break;
        }
    }

    if (error_flag)
    {
        return nullptr;
    }

    SonarDriver *instance = new SonarDriver(example_param, example_flag);

    if (instance == nullptr)
    {
        PX4_ERR("alloc failed");
    }

    return instance;
}

SonarDriver::SonarDriver(int example_param, bool example_flag)
    : ModuleParams(nullptr)
{
}
int SonarDriver::init()
{

// V6C
// CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
// CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6"
// CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5"
// CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
// CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
	int ret = 0;
	do { // create a scope to handle exit conditions using break

		// open fd
		_fd = ::open((char *)"/dev/ttyS1", O_RDWR | O_NOCTTY);

		if (_fd < 0) {
			PX4_ERR("Error opening fd");
			return -1;
		}

		// baudrate 115200, 8 bits, no parity, 1 stop bit
		unsigned speed = B9600;
		termios uart_config{};
		int termios_state{};

		tcgetattr(_fd, &uart_config);

		// clear ONLCR flag (which appends a CR for every LF)
		uart_config.c_oflag &= ~ONLCR;

		// set baud rate
		if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
			PX4_ERR("CFG: %d ISPD", termios_state);
			ret = -1;
			break;
		}

		if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
			PX4_ERR("CFG: %d OSPD\n", termios_state);
			ret = -1;
			break;
		}

		if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
			PX4_ERR("baud %d ATTR", termios_state);
			ret = -1;
			break;
		}

		uart_config.c_cflag |= (CLOCAL | CREAD);	// ignore modem controls
		uart_config.c_cflag &= ~CSIZE;
		uart_config.c_cflag |= CS8;			// 8-bit characters
		uart_config.c_cflag &= ~PARENB;			// no parity bit
		uart_config.c_cflag &= ~CSTOPB;			// only need 1 stop bit
		uart_config.c_cflag &= ~CRTSCTS;		// no hardware flowcontrol

		// setup for non-canonical mode
		uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
		uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
		uart_config.c_oflag &= ~OPOST;

		// fetch bytes as they become available
		uart_config.c_cc[VMIN] = 1;
		uart_config.c_cc[VTIME] = 1;

		if (_fd < 0) {
			PX4_ERR("FAIL: laser fd");
			ret = -1;
			break;
		}
	} while (0);
    return ret;
}
void SonarDriver::run()
{
    PX4_INFO("_fd=%d\n",_fd);
    if(_fd<0)
    {
   init();
    }
    PX4_INFO("_fd=%d\n",_fd);
// char readbuf[sizeof(_linebuf)] {};
// 	unsigned readlen = sizeof(readbuf) - 1;
// char data;
    while(!should_exit())
    {
        int bytes_available = 0;
	::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);

// 	if (!bytes_available) {
// continue;
// 	}
PX4_INFO("bytes_available=%d\n",bytes_available);
// for(int i=0;i<bytes_available;i++)
// {
//     ret = ::read(_fd, &readbuf[0], readlen);

// 		if (ret < 0) {
// 			PX4_ERR("read err: %d", ret);
// continue;
// 		}
//         PX4_INFO("data=%x\n",readbuf[i]);
// }

// ret = ::read(_fd, &data, 1);
// PX4_INFO("data=%x\n",data);
// PX4_INFO("ret=%d\n",ret);
  usleep(50000);
    }
}

int SonarDriver::print_usage(const char *reason)
{
    if (reason)
    {
        PX4_WARN("%s\n", reason);
    }

    PRINT_MODULE_DESCRIPTION(
        R"DESCR_STR(
### Description
Section that describes the provided module functionality.

This is a template for a module running as a task in the background with start/stop/status functionality.

### Implementation
Section describing the high-level implementation of this module.

### Examples
CLI usage example:
$ module start -f -p 42

)DESCR_STR");

    PRINT_MODULE_USAGE_NAME("module", "template");
    PRINT_MODULE_USAGE_COMMAND("start");
    PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
    PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true);
    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

    return 0;
}

int sonar_driver_main(int argc, char *argv[])
{
    return SonarDriver::main(argc, argv);
}


